Copyright □ 1999, by Professor Jong-Hwan Kim, Korea Advanced Institute of Science and Technology (KAIST).  All rights reserved. Reproduction or translation of any part of this work is unlawful. Request for permission or further information should be addressed to Professor Jong-Hwan Kim at <johkim@vivaldi.kaist.ac.kr> or <Tel: +82-42-869-3448>.

Prof. Jong-Hwan Kim	Intelligent Control Lab, KAIST

11.로봇 제어 프로그램 Ⅴ

실제 축구에서와 마찬가지로 로봇축구에서도 골키퍼의 역할이 매우 중요하다. 여기서는 수비의 대부분을 차지하는 골키퍼함수와 로봇이 벽에 걸려 움직이지 못할 때 사용하는 벽처리 함수에 대해서 알아본다. 프로그램 설명은 해당되는 부분에 부분별로 나타내었다.

11.1 Goalie 함수

골키퍼의 역할은 골 영역에 들어온 공을 걷어내고, 상대방 로봇이 슛한 공을 막아내는 것이다. 공격수 로봇들이 공격을 잘한다 하더라고 골키퍼가 공을 막아내지 못하면 전체 로봇 축구 시스템의 성능은 떨어진다. 그러므로 골키퍼의 역할은 전체 로봇 축구 시스템에서 대단히 중요하다. 이런 골키퍼 함수를 어떻게 구현하는지에 대해서 알아본다. 여기서 보이는 것은 하나의 예이고 다른 방법으로도 구현할 수 있을 것이다.

그림 11.1은 공의 위치에 따라서 골키퍼가 취해야 하는 행동을 결정하기 위하여 골과 공 사이의 거리를 세 단계로 나누었다.

그림 11. 골과 공사이의 거리 구분

즉, 공이 골대에서 멀리 있을 때, 중간거리에 있을 때, 가까이 있을 때에 따라 골키퍼의 움직임은 달라야 한다. 기본적으로 공이 골대에서 멀리 있을 때는 골키퍼는 골대영역의 중앙에 위치한다. 그러다가 공이 골대영역으로 다가올 때 공의 위치에 따라서 골키퍼는 움직여야 한다. 좀 더 정밀한 제어를 위해서는 공이 멀리서부터 골대영역으로 빨리 다가올 때 공의 도착지점을 예측하여 미리 그 지점으로 골키퍼가 이동해 있어야 한다. 여기서는 공의 이동궤적을 예측하는 것은 생략하겠다. 기본적으로 골키퍼는 골대영역에 평행한 직선으로 움직이면 된다. 즉, 골대에서 X축으로 몇 cm 떨어진 위치에서 그 선을 따라서 공의 상태 및 위치에 따라서 Y축 방향으로 움직이면 된다. 먼저 공의 상태에 따른 골키퍼의 움직임에 대해서 살펴보자.

그림 11.2 공이 골대영역에서 멀리 위치할 때

그림 11.2는 공이 골대영역에서 멀리 있을 때 골키퍼가 이동해가야 하는 지점을 나타낸다. 처음에 골키퍼는 골대영역과 평행한 직선 상에서 골대영역의 중앙에 위치한다. 즉, 골키퍼의 초기 위치는 (estimate_x cm, 65cm) 이다. estimate_x는 프로그램에서 상수 값으로 설정하며, 그 상수 값을 G_OFFSET으로 정하자. 그리고 경기장의 경계선을 정하는 것에 따라서 좌표계가 조금씩 이동하므로 이 값은 경기장의 경계선을 정할 때 조금씩 조절해야 한다.  65cm는 Y 좌표계에서 경기장크기의 절반인 값이며, 15cm는 X 좌표계에서 골대영역의 값이다. 공이 멀리서 다가오므로 공의 Y 좌표를 따라서 많이 움직일 필요는 없고, 그림 11.2에 나타낸 원리로 초기 위치인 골대 영역 중앙에서 약간만 이동하면 된다. 그림 9.2에서 dx와 dy는 다음과 같다.

[eqn: rm &dx = Ball_x - 15# rm &dy = Ball_y - 65]

(11.)

여기서 [eqn: rm Ball_x 와 ~Ball_y]는 각각 공의 X, Y 좌표값이다.

식 (11.1)과 삼각형의 비례식을 이용하면 로봇이 이동할 y 값은 다음과 같이 구할 수 있다.

[eqn: &rm dx : dy = x : y# &rm y = dy over dx x]

(11.2)

여기서 x는 15-G_OFFSET이 된다. 로봇이 이동해가야 하는 Y 좌표는 estimate_y = 65 + y 가 된다. 공이 먼 거리에서 골대영역으로 다가올 때 로봇이 위치해야 하는 좌표는 (estimate_x, estimate_y) = (G_OFFSET, 65 + y)가 된다. 즉, 공이 먼거리에 있을 때 골키퍼 로봇은 골대 중앙에 놓이게 되고 공의 위치에 따라서 약간씩만 움직인다.

그림 11.3은 공이 중간거리에 있을 때를 나타낸다.

그림 11.3 공이 골대영역에서 중간거리에 위치할 때

공이 먼 거리에 있다가 중간거리내로 들어오면 로봇은 공의 Y 좌표값을 따라서 이동한다. 즉, 골키퍼가 이동해야 하는 좌표는 (estimate_x, estimate_y) = (G_OFFSET, PositionOfBall[1]) 이다. 여기서 PositionOfBall[1]은 공의 Y 좌표값이다.

공이 골대영역 근처에 위치할 때를 그림 11.4에서 나타내었다.

(a) 공이 골대영역 가까이            (b) 공이 골대영역 내부에

위치할 때	위치할 때

그림 11.4 공이 골대영역 근처에 있을 때

여기서 다시 공이 골대영역 가까이 위치할 때와 공이 골대영역 내부에 위치할 때의 두 가지로 나누었다. 공이 골대영역 가까이 위치할 때는 공이 중간거리에 위치할 때와 마찬가지로 공의 Y 좌표를 따라서 움직이면 된다. 그러나 공이 골대영역에 들어오면 골키퍼는 공을 차내야 하므로 이 경우에 골키퍼는 공을 향해 이동한다. 이 경우 골키퍼가 이동해야 하는 좌표는 (estimate_x, estimate_y) = (PositionOf Ball[0], PositionOfBall[1]) 이다. 하지만 골키퍼는 골대영역 내에서 움직여야 한다. 공의 Y 좌표가 골대와 멀리 떨어져 있을 때는 골대영역 경계선으로 이동하면 된다. 즉, 골키퍼가 이동할 Y 값의 상한과 하한의 좌표를 정해야 한다. 이런 방법으로 골키퍼를 움직이게 하면 불필요한 움직임을 줄일 수 있다. 이러한 위쪽과 아래쪽 경계지점을 그림 11.5에 나타내었다.

그림 11.5 골키퍼가 움직일 경계선 설정

위에서 설명한 방법을 프로그램으로 나타내면 다음과 같고, 이 부분을 블록(Block) 1이라 하자.

#define G_OFFSET	6.0	// 골키퍼가 움직일 x 좌표값

#define G_ANGLE_BOUND	60	// 골키퍼의 예외처리를 위한 각도 크기

#define G_BOUND	8.0	// 골키퍼의 예외처리를 위한 x 좌표값

#define G_LENGTH	40	// 골대 길이

void Goalie(int whichrobot)

{

double estimate_x,estimate_y, dx, dy;

int angle, dir=1, r_angle;	// 변수 설정

if(PositionOfBall[0] > 60)	// 공의 거리가 멀 때

{

estimate_x = 0.0 + G_OFFSET;	// 골키퍼가 움직일 x 좌표값

estimate_y = 130.0/2.0 -((130.0/2.0 - PositionOfBall[1])/(PositionOfBall[0]-15) *(15-G_OFFSET) );	// 골키퍼가 움직일 y 좌표값, 식 (11.2)를 이용

}

else if(PositionOfBall[0] > 15)	// 공의 거리가 중간일 때

{

estimate_x = 0.0 + G_OFFSET;	// 골키퍼가 움직일 X 좌표값

estimate_y = PositionOfBall[1];	// 골키퍼가 움직일 Y 좌표값(=공의 Y 좌표값)

}

else	// 공의 거리가 가까울 때

{

estimate_x = 0.0 + G_OFFSET;	// 골키퍼가 움직일 X 좌표값

estimate_y = PositionOfBall[1];	// 골키퍼가 움직일 Y 좌표값

}

if(fabs(PositionOfBall[0]-PositionOfHomeRobot[whichrobot][0])<4.0 && PositionOfBall[0]<10)

{	// 공이 골대영역에 들어섰을 때

estimate_x = PositionOfBall[0];

estimate_y = PositionOfBall[1];	// 골키퍼가 이동할 좌표는 공의 좌표가 된다.

}

if( estimate_y > (65.0+G_LENGTH/2.0) )	estimate_y = 65.0+G_LENGTH/2.0+4.0;

// 골키퍼가 이동하는 Y 좌표값의 최대값 설정

if( estimate_y < (65.0-G_LENGTH/2.0) )  estimate_y = 65.0-G_LENGTH/2.0-4.0;

// 골키퍼가 이동하는 Y 좌표값의 최소값 설정

이상으로 공의 위치에 따라서 골키퍼 로봇이 이동해 가야할 지점 (estimate-x, estimate_y)들을 구하였다. 다음은 이러한 지점으로 골키퍼를 이동시키기 위한 방법에 대하여 알아본다. 위에서 구한 지점으로 골키퍼를 이동시키기 위해 GPosition 함수를 이용한다. 이 함수는 Posture 함수와 구조가 같으나 제어 계수들의 값이 조금 다르다. GPosition 함수를 이용한다 하더라도 이동할 지점과 골키퍼 로봇의 거리가 멀 때는 직진 이동 제어가 쉽지 않으므로 X 좌표값이 G_OFFSET인 직선 위를 따라서 곧바로 움직이지 못하는 경우가 있다. 이를 해결하기 위해서 골키퍼가 이동할 목표지점을 현재 골키퍼의 위치에서 조금씩 증가 또는 감소시킨 값으로 정한다. 즉, 이동할 Y 좌표값이 현재 골키퍼의 Y 좌표 값보다 큰 경우에는 이동좌표를 현재 골키퍼 로봇의 Y 좌표값보다 조금 큰 값으로 정한다. 이동할 Y 좌표값이 골키퍼의 Y 좌표값보다 작은 경우에는 골키퍼가 이동할 목표지점을 현재 골키퍼의 Y 좌표값보다 조금 작게 지정한다. 이렇게 함으로써 골키퍼 로봇이 직선을 따라서 곧바로 이동하게 된다. 그리고 골키퍼가 목표지점 근처에 도달했을 경우에는 그 위치에서 [eqn: +-90 DEG]의 각을 유지하면서 머물러 있으면 된다. 이를 위해서는 로봇의 각을 유지하게끔 하는 함수 Angle을 이용한다. 그리고 골키퍼로봇이 공보다 바로 앞쪽에 위치할 때는 자살골을 방지하기 위해서 멈추게 한다. 위에서 설명한 것에 대해서 다음과 같이 프로그래밍하였고, 이를 블록 2라 하자.

r_angle = AngleOfHomeRobot[whichrobot];	// 골키퍼 로봇의 각 측정

while(r_angle > 180)	r_angle -= 360;	// 골키퍼 로봇의 각도를 [eqn: +-180 DEG]이내로 제한

while(r_angle < -180) r_angle += 360;

if( ( (r_angle>40)&&(r_angle<180-40)&&(estimate_y>PositionOfHomeRobot[whichrobot][1]) ) || ( (r_angle>-130)&&(r_angle<-40)&&(estimate_y>PositionOfHomeRobot[whichrobot][1]) ) )

// 골키퍼가 이동할 Y좌표가 골키퍼의 Y좌표보다	클 때

GPosition(whichrobot, estimate_x, PositionOfHomeRobot[whichrobot][1]+2.5);

// 현재 위치에서 +2.5cm Y좌표로 이동

else if( ( (r_angle>40)&&(r_angle<180-40)&&(estimate_y<PositionOfHomeRobot[whichrobot][1]) ) || ( (r_angle>-130)&&(r_angle<-40)&&(estimate_y<PositionOfHomeRobot[whichrobot][1]) ) )

// 골키퍼가 이동할 Y좌표가 골키퍼의 Y좌표보다	작을 때

GPosition(whichrobot, estimate_x, PositionOfHomeRobot[whichrobot][1]-2.5);

// 현재 위치에서 -2.5cm Y좌표로 이동

if(PositionOfBall[0]<PositionOfHomeRobot[whichrobot][0] && fabs(PositionOfBall[1]- PositionOfHome Robot[whichrobot][1])<4.0)	// 골키퍼가 공의 바로 앞쪽에 있을 때

WhichRobotStop(whichrobot);	// 자살골 방지를 위해 골키퍼를 정지 함

dx = estimate_x - PositionOfHomeRobot[whichrobot][0];	// X 좌표에서 골키퍼가 이동할 좌표와	골키퍼 위치와의 거리차

dy = estimate_y - PositionOfHomeRobot[whichrobot][1];	// Y 좌표에서 골키퍼가 이동할 좌표와

골키퍼 위치와의 거리차

if(dx==0 && dy==0)	angle = 90;	// 골키퍼가 이동할 좌표에서 각도 계산

else	angle = (int)(180/M_PI*atan2((double)(dy),(double)(dx)));

while(angle >180)  angle -= 360, dir = -1;	// 구한 각도가 [eqn: 180 DEG]보다 클 때 -[eqn: 180 DEG]이내로

제한하고 방향을 설정하는 변수에 반대방향임을	저장(-1), 이를 나중에 Angle 함수에 적용

while(angle <-180) angle += 360, dir = -1;	// 구한 각도가 -[eqn: 180 DEG]보다 클 때 [eqn: 180 DEG]이내로

제한하고 방향을 설정하는 변수에 반대방향임을

저장(-1), 이를 나중에 Angle 함수에 적용

if( (dx*dx+dy*dy < 1.6*1.6) && (fabs(dx)<2.2) && fabs(dy)<2.0 )

// 골키퍼가 이동할 거리가 1.6cm 이내이고

X 좌표차가 2.2cm 이내이고, Y 좌표차가 2cm 이내일 때

Angle(whichrobot, dir*90);	// 골키퍼 로봇의 방향에 따라서 [eqn: +-90 DEG]각도를 유지

지금까지 보인 방법은 하나의 예이고 다른 점들도 고려될 수 있다. 그리고 위에서 공의 거리를 먼 거리, 중간거리, 근접거리로 나누었는데 이런 기준도 설계자의 의도에 알맞게 설정해야 한다. 예제 프로그램에서는 골키퍼 로봇이 이동하기 위해서 GPosition 함수를 사용한다. 이 함수의 프로그램은 Posture 함수와 비슷하다.

위에서 설명한 것 이외에도 골키퍼로봇이 경기장의 벽에 걸렸을 때 이를 빠져 나오게 하는 함수를 만들어야 한다. 그림 11.6은 골키퍼가 골대영역에서 벽에 걸렸을 때 처리하는 방법을 나타낸다. 크게 골키퍼가 골대 안에 들어가서 못 나올 경우와 골대영역의 벽에 걸려서 못 움직이는 경우가 있다. 그림에서 화살표는 로봇이 이동해야 하는 방향이다. 골키퍼가 골대영역의 벽에 걸렸을 때는 골키퍼의 각을 조사하여 각 상황에 맞게 로봇을 움직이면 되고, 골키퍼가 골대영역 내부에서 못 빠져 나올 때는 골대영역 중심으로 이동하면 된다.

이를 프로그램으로 구현하면 다음과 같고, 이를 블록 3이라 하자. 이에 대한 좀더 자세한 설명은 다음절의 벽처리 함수인 AvoidBound 함수에서 한다.

그림 11.6 골키퍼 움직임의 예외처리

while(AngleOfHomeRobot[whichrobot] >180)  AngleOfHomeRobot[whichrobot] -= 360;

while(AngleOfHomeRobot[whichrobot] <-180) AngleOfHomeRobot[whichrobot] += 360;

// 골키퍼 로봇의 각도를 [eqn: +-180 DEG]이내로 제한

if(PositionOfHomeRobot[whichrobot][0] < 0.0)	// 골키퍼로봇이 골대 안에 위치할 경우

GPosition(whichrobot, G_OFFSET, 45.0);	// 골대영역의 중앙으로 이동

// 그림 11.6의 ①의 경우

else

{

angle = abs(AngleOfHomeRobot[whichrobot]);	// 골키퍼로봇의 절대 각도를 저장

if(PositionOfHomeRobot[whichrobot][1]<65.0)	// 골키퍼로봇의 Y좌표가 65cm 보다	작을 때

{

if(angle > 180 - G_ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0 + G_BOUND)	// 골키퍼 로봇의 각도가 [eqn: 180DEG]근처이고	벽에 붙어있을 때, 그림 11.6의 ②의 경우

Velocity(whichrobot, -17, -1);	// 왼쪽 바퀴를 뒤쪽 방향으로 빨리 돌림

else if(angle < G_ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0 + G_BOUND)	// 골키퍼 로봇의 각도가 [eqn: 0 DEG]근처이고

벽에 붙어있을 때, 그림 11.6의 ③의 경우

Velocity(whichrobot, 1, 17);	// 오른쪽 바퀴를 앞쪽으로 빨리 돌림

}

else	// 골키퍼로봇의 Y좌표가 65cm 보다 클 때

{

if(angle > 180 - G_ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0 + G_BOUND)	// 골키퍼 로봇의 각도가 [eqn: 180DEG]근처이고

벽에 붙어있을 때, 그림 11.6의 ④의 경우

Velocity(whichrobot, -1, -17);	// 오른쪽 바퀴를 뒤쪽으로 빨리 돌림

else if(angle < G_ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0 + G_BOUND)	// 골키퍼 로봇의 각도가 [eqn: 0 DEG]근처이고	벽에 붙어있을 때, 그림 11.6의 ⑤의 경우

Velocity(whichrobot, 17, 1);	// 왼쪽 바퀴를 앞쪽으로 빨리 돌림

}

}

다음은 위에서 설명한 함수들을 종합하여 골키퍼를 구현한 Goalie 함수이다. 내부의 블록 1, 2, 3은 앞에서 설명하였다.

#define G_OFFSET	6.0	// 골키퍼가 움직일 X 좌표값

#define G_ANGLE_BOUND	60	// 골키퍼의 예외처리를 위한 각도 크기

#define G_BOUND	8.0	// 골키퍼의 예외처리를 위한 X 좌표값

#define G_LENGTH	40	// 골대 길이

void Goalie(int whichrobot)

{

double estimate_x,estimate_y, dx, dy;

int angle, dir=1, r_angle;	// 변수 설정

블록 1;	// 골키퍼가 이동할 위치 지정

블록 2;	// 골키퍼로봇의 위치제어

블록 3;	// 골키퍼로봇의 예외처리 함수

}

지금까지 설명한 것은 골키퍼를 구현하는 하나의 예이다. 그러므로 다른 방법으로 골키퍼 로봇의 움직임을 구현할 수 있다. 그리고 실제 공은 매우 빨리 움직일 경우도 있으므로 골키퍼의 정확한 움직임을 위해서는 공의 이동궤적을 추정하기 위한 예측기도 필요하다.

11.2 벽처리 함수 - AvoidBound

로봇들이 경기장 위를 움직이다 보면 벽에 걸려서 못 움직이는 경우가 많다. 이를 위해서 로봇이 벽에 걸렸을 때 처리하는 함수에 대해 알아본다. 로봇이 벽에 걸리는 경우는 경기장 벽의 위쪽과 아래쪽, 왼쪽, 오른쪽의 네 가지로 나눌 수 있다. 이는 골키퍼 로봇이 골 안에 걸렸을 때 처리하는 방법과 같다. 여기서는 이를 하나씩 좀 더 자세히 살펴본다.

그림 11.7은 로봇이 위쪽과 아래쪽 벽에 걸렸을 때 로봇이 이동할 방향을 나타낸다. 로봇이 경기장 위쪽 벽에 걸렸을 때 로봇의 방향각이 [eqn: +-90DEG]근처일 경우 로봇을 공 쪽으로 방향을 돌린다. 로봇이 아래쪽 벽에 걸렸을 때도 마찬가지이다.

그림 11.7 로봇이 위쪽 아래쪽 벽에 걸렸을 때

그림 11.7의 ①은 로봇이 아랫방향 (-Y 방향)으로 향하고 있으며, 즉[eqn: theta_r]= -[eqn: 90DEG], 그림 11.7의 ②는 윗방향 (+Y 방향), 그러나 로봇은 양방향으로 이동 가능하므로 [eqn: theta_r] = -[eqn: 90DEG], 그림 11.7의 ③은 아랫방향 (-Y 방향), 그러나 로봇은 양방향으로 이동 가능하므로 [eqn: theta_r] = [eqn: 90DEG], 그림 11.7의 ④는 윗 방향 (+Y 방향), 즉 [eqn: theta_r] = [eqn: 90DEG]이다. 모든 경우에 로봇은 공 쪽으로 방향을 돌리도록 한다. 로봇은 양방향으로 이동이 가능하므로 ①의 경우는 원하는 방향각[eqn: theta_d = 0 DEG]이고, ②의 경우는[eqn: theta_d = 180 DEG], ③의 경우는[eqn: theta_d = 0 DEG], ④의 경우는[eqn: theta_d =] [eqn: 180 DEG]이다. 로봇이 벽에서 떨어져 공 쪽으로 방향을 틀기 위해서는 로봇중심 속도[eqn: v_c]를 상수 값으로 유지한 상태에서, 로봇의 방향각 [eqn: theta_r]과 원하는 방향각 [eqn: theta_d] 사이의 각도 오차[eqn: theta_e]를 이용하여 로봇을 공쪽으로 이동시킬 수 있다. 이를 식 (11.3)에 나타내었다.

[eqn: & v_R ~=~v_c ~+ K_\angle CDOT theta_e# & v_L ~=~v_c ~- K_\angle CDOT theta_e# & theta_e ~= ~theta_d ~ - theta_r]

(11.3)

식 (11.3)처럼 [eqn: v_c]를 상수값으로 설정하면 로봇은 벽에서 약간 떨어지면서 방향을 틀어 공 쪽으로 움직인다. 실제로 로봇이 벽에 걸렸을 때는 로봇의 방향각이 정확히[eqn: 90DEG]일 경우뿐만 아니라,[eqn: 90DEG]에서 어느 정도 벗어나 있을 때도 포함된다. 이를 위해서 각도 범위값인[eqn: theta_bound]를 설정할 필요가 있다.이를 그림 11.8에서 나타내었다. 그리고 로봇이 벽에 걸렸을 때를 판별하려면 이런 각도의 범위값 뿐만 아니라 벽으로부터의 거리의 범위값도 필요하다. 그림 11.8은 로봇이 거리와 방향각의 범위값들을 가지고 경기장의 위아래, 왼쪽 오른쪽 벽에 걸렸을 때를 나타낸다. 이를 프로그램하면 다음과 같고, 이를 블록 1이라 하자.

그림 11.8 로봇이 각도와 거리 범위내에서 벽에 걸렸을 때

#define ANGLE_BOUND 60	// 각도 범위 선언

#define DISTANCE_BOUND 9	// 거리 범위 선언

int angle_r,angle,theta_e, vl, vr;	// 변수 선언

double dx, dy;

dx=PositionOfBall[0]-PositionOfHomeRobot[whichrobot][0];	// 공과 로봇간의 X 거리 차

dy=PositionOfBall[1]-PositionOfHomeRobot[whichrobot][1];	// 공과 로봇간의 Y 거리 차

if(dx==0 && dy==0)	theta_e = 90;	// 공과 로봇이 이루는 각 계산

else	theta_e = (int)(180/M_PI*atan2((double)(dy),(double)(dx)));

angle_r = AngleOfHomeRobot[whichrobot];	// 로봇의 각도 계산

theta_e -= AngleOfHomeRobot[whichrobot];	// 공과 로봇이 이루는 각과 로봇방향각	과의 각도 오차 계산

while(angle_r>180) angle_r -= 360;	// 로봇의 각도를 [eqn: +-180DEG]이내로 제한

while(angle_r<-180) angle_r += 360;

while(theta_e >180)  theta_e -= 360;	// 각도 오차를 [eqn: +-180DEG]이내로 제한

while(theta_e <-180) theta_e += 360;

// Top and Bottom side

if(angle_r> -90-ANGLE_BOUND && angle_r < -90+ANGLE_BOUND && PositionOfHomeRobot [whichrobot][1] > 130.0 - DISTANCE_BOUND ){	// 그림 11.7의 ①의 경우

vr = (int)(14 +19.0/90.0*theta_e);	// 양의 상수값 [eqn: v_c], 식 (11.3) 이용

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle_r > 90-ANGLE_BOUND && angle_r < 90+ANGLE_BOUND && PositionOfHomeRobot [whichrobot][1] > 130.0 - DISTANCE_BOUND ){	// 그림 11.7의 ②의 경우

vr = (int)(-14 +19.0/90.0*theta_e);	// 음의 상수값 [eqn: v_c], 식 (11.3) 이용

vl = (int)(-14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle_r > -90-ANGLE_BOUND && angle_r < -90+ANGLE_BOUND && PositionOfHome Robot[whichrobot][1] < 0.0 + DISTANCE_BOUND ){	// 그림 11.7의 ③의 경우

vr = (int)(-14 +19.0/90.0*theta_e);	// 음의 상수값 [eqn: v_c], 식 (11.3) 이용

vl = (int)(-14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle_r > 90-ANGLE_BOUND && angle_r < 90+ANGLE_BOUND && PositionOfHomeRobot [whichrobot][1] < 0.0 + DISTANCE_BOUND ){	// 그림 11.7의 ④의 경우

vr = (int)(14 +19.0/90.0*theta_e);	// 양의 상수값 [eqn: v_c], 식 (11.3) 이용

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

그림 11.9는 로봇이 왼쪽 벽에 걸렸을 때 로봇이 이동할 방향을 나타낸다. 여기서도 로봇의 제어 입력인 오른쪽과 왼쪽 바퀴속도는 식 (11.3)을 이용한다. 이 경우에도 거리와 각도 범위값이 존재한다.

그림 11.9 로봇이 왼쪽 벽에 걸렸을 경우

이를 프로그램하면 다음과 같고, 이를 블록 2라고 하자.

//Left side

angle = abs(angle_r);	// 로봇 방향각의 절대값을 계산

if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0+DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){	// 그림 11.9의 ①의 경우

vr = (int)(14 +19.0/90.0*theta_e);

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0] < 0.0+ DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){	// 그림 11.9의 ②의 경우

vr = (int)(-14 +19.0/90.0*theta_e);

vl = (int)(-14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]< 0.0+DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){	// 그림 11.9의 ③의 경우

vr = (int)(14 +19.0/90.0*theta_e);

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]<0.0+ DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){	// 그림 11.9의 ④의 경우

vr = (int)(-14 +19.0/90.0*theta_e);

vl = (int)(-14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

그림 11.10은 로봇이 오른쪽 벽에 걸렸을 때 로봇이 이동할 방향을 나타낸다. 마찬가지로 제어입력은 식 (11.3)을 이용한다. 이를 프로그램하면 다음과 같고, 이를 블록 3이라 하자.

그림 11.10 로봇이 오른쪽 벽에 걸렸을 경우

//Right side

if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){	// 그림 11.10의 ①의 경우

vr = (int)(-14 +19.0/90.0*theta_e);

vl = (int)(-14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0- DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] > 85.0){	// 그림 11.10의 ②의 경우

vr = (int)(14 +19.0/90.0*theta_e);

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle < ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0-DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){	// 그림 11.10의 ③의 경우

vr = (int)(-14 + 19.0/90.0*theta_e);

vl = (int)(-14 - 19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

else if(angle > 180-ANGLE_BOUND && PositionOfHomeRobot[whichrobot][0]>150.0- DISTANCE_BOUND && PositionOfHomeRobot[whichrobot][1] < 45.0){	// 그림 11.10의 ④의 경우

vr = (int)(14 +19.0/90.0*theta_e);

vl = (int)(14 -19.0/90.0*theta_e);

Velocity(whichrobot, vl, vr);

}

위에서 설명한 프로그램들을 묶으면 다음의 벽처리 함수를 구현할 수 있다.

#define ANGLE_BOUND 60	// 각도 범위 선언

#define DISTANCE_BOUND 9	// 거리 범위 선언

void AvoidBound(int whichrobot)	// 벽처리 함수

{

블록 1;	// 로봇이 위아래 벽에 걸렸을 경우

블록 2;	// 로봇이 왼쪽 벽에 걸렸을 경우

블록 3;	// 로봇이 오른쪽 벽에 걸렸을 경우

}